# 与原版功能一致完全不需要修改。
# 作用就是启动server作为与server的接口。保存状态，产生回报。
import os
import time
import random
import math
import hfo
import logging
import numpy as np
module_logger = logging.getLogger("mainModule.hfo_game")

# 一些全局变量
# config_dir = HFODir + "/bin/formations-dt" 
# Directory containing HFO config files.
gui = False # Open a GUI window.
log_game = False # Log the HFO game.
server_addr = "localhost" # Address of rcssserver.
team_name = "base_left" # Name of team for agents.
play_goalie = False # Should the agent play goalie.
record_dir = "" # Directory to record states,actions,rewards.
ball_x_min = 0 # Ball X-Position initialization minimum.
ball_x_max = 0.2 # Ball X-Position initialization maximum.
offense_on_ball = 1 # give the ball to Offensive player 0
verbose = False # Server prints verbose output.
kPassVelThreshold = -.5

def StartHFOServer(port, offense_agents, offense_npcs, defense_agents, defense_npcs, HFODir):
  server_cmd = "python3 " + HFODir + "/bin/HFO --fullstate --frames-per-trial 300" 
  cmd = server_cmd + " --port " + port + " --offense-agents " + offense_agents \
      + " --offense-npcs " + offense_npcs + " --defense-agents " + defense_agents \
      + " --defense-npcs " + defense_npcs + " --ball-x-min " + str(ball_x_min) \
      + " --ball-x-max " + str(ball_x_max) + " --offense-on-ball " + str(offense_on_ball)
    # Command executed to start the HFO server.
  if not gui:
    cmd += " --headless"
  if not log_game:
    cmd += " --no-logging"
  if verbose:
    cmd += " --verbose"
  cmd += " &"
  # 进程在后台操作（多进程）
  os.system(cmd)
  # LOG(INFO) << "Starting server with command: " << cmd;
  # CHECK_EQ(system(cmd.c_str()), 0) << "Unable to start the HFO server.";
  time.sleep(10)

def StartDummyTeammate(port):
  cmd = "./bin/dummy_teammate " + port + " > /dev/null"
  # LOG(INFO) << "Starting dummy teammate with command: " << cmd;
  os.system(cmd)
  # CHECK_EQ(system(cmd.c_str()), 0) << "Unable to start dummy teammate.";
  time.sleep(5)

def StartDummyGoalie(port):
  cmd = "./bin/dummy_goalie " + port + " > /dev/null"
  # LOG(INFO) << "Starting dummy goalie with command: " << cmd;
  os.system(cmd)
  # CHECK_EQ(system(cmd.c_str()), 0) << "Unable to start dummy goalie.";
  time.sleep(5)

def StartChaser(port):
  cmd = "./bin/chaser " + port + " " + team_name + " " + goalie + " > /dev/null"
  # LOG(INFO) << "Starting chaser with command: " << cmd;
  os.system(cmd)
  # CHECK_EQ(system(cmd.c_str()), 0) << "Unable to start chaser.";
  time.sleep(5)

def StopHFOServer():
    os.system("killall -9 rcssserver")

# def ConnectToServer(hfo_env, port):
#   hfo.connectToServer(LOW_LEVEL_FEATURE_SET, config_dir, port, server_addr, team_name, play_goalie, record_dir)
#   time.sleep(5)

def GetRandomHFOAction():
  # at array类整数
  # ap 元组，元祖元素为array类，每个array类包含一个元祖
  action_indx = 0              #0 represent dash,1 represent turn,2 represent tackle,3 represent kick
  arg1 = np.array([random.uniform(-100,100)]) #arg1 represent dash power,negative values move backwards
  arg2 = np.array([random.uniform(-180,180)]) #arg2 represent dash direction,0 represent dashing forward
                                                       # 90 represent dashing to the agent's right side
  arg3 = np.array([random.uniform(-180,180)]) #agent Turn to arg3 degree
  arg4 = np.array([random.uniform(-180,180)]) # arg4 represent tackle degree
  arg5 = np.array([random.uniform(0,100)])    #arg5 represent kick power
  arg6 = np.array([random.uniform(-180,180)]) #arg6 represent kick degree
  return np.array(action_indx), [arg1, arg2, arg3, arg4, arg5, arg6]

#  Get the offset for a param of a given action. Returns -1 if a
#  non-existant offset is requested.
def GetParamOffset(action, arg_num = 0) :
  if arg_num < 0 or arg_num > 1:      #arg_num=0 or 1
    return -1                  #value returned range from 0 to 5 represent arg1....arg6 in function GetRandomHFOAction
  if action == hfo.DASH:
    return arg_num
  elif action == hfo.TURN:
    if arg_num == 0:
      return 2
    else:
      return -1
  elif action == hfo.TACKLE:
    if arg_num == 0:
      return 3
    else:
      return -1
  elif action == hfo.KICK:
    return 4 + arg_num


def SelectAction(action, action_param, epsilon):
  if random.random() < epsilon:
    # 这个有问题不知道怎么设置其他的问题
    return GetRandomHFOAction()
  else:
    return GetAction(action, action_param)

def GetAction(action, action_param):

  arg1 = action_param[GetParamOffset(action,0)]
  arg2offset = GetParamOffset(action,1)
  if arg2offset < 0:
    arg2 = 0
  else:
    arg2 = action_param[arg2offset]
  return [action, arg1, arg2]


class HFOGameState(object):
  """
  封装好的，用来处理hfo的信息
  hfo环境第一层：

  hfostate = hfo.HFOEnvironment()
  features = hfostate.getState()    // 这样就可以获得到所有的features
  status = hfostate.step()          //  status保存hfo这一个状态下，step后的后状态（用来判断循环）

  HFOGameState是封装的新的class
  在这个class里面我们定义了reward()，update()

  详细参数见manual
  """
  def __init__(self, unum=0):
    self.old_ball_prox = 0
    self.ball_prox_delta = 0
    self.old_kickable = 0
    self.kickable_delta = 0
    self.old_ball_dist_goal = 0
    self.ball_dist_goal_delta = 0
    self.steps = 0
    self.total_reward = 0
    self.extrinsic_reward = 0
    self.status = hfo.IN_GAME
    self.episode_over = False
    self.got_kickable_reward = False
    self.our_unum = unum
    self.pass_active = False
    self.old_player_on_ball = 0
    self.player_on_ball = 0
  #VLOG(1) << "Creating new HFOGameState";

  def update(self, hfo_env):
    self.status = hfo_env.step()
    self.episode_over = False
    # module_logger.debug(self.status)
    if self.status == hfo.SERVER_DOWN:
        print("Server Down!")
     #LOG(FATAL) << "Server Down!";
      #exit(1);
    elif self.status != hfo.IN_GAME:
      self.episode_over = True
    current_state = hfo_env.getState()
    # state_data = np.zeros(self.getStateSize(), dtype=np.float32)

    self.ball_proximity = current_state[53]
    goal_proximity = current_state[15]
    ball_dist = 1.0 - self.ball_proximity
    goal_dist = 1.0 - goal_proximity
    # 因为proximity是接近的距离
    # 1表示紧挨着，-1表示在对角
    
    kickable = current_state[12]
    ball_ang_sin_rad = current_state[51]
    ball_ang_cos_rad = current_state[52]
    ball_ang_rad = math.acos(ball_ang_cos_rad)
    if ball_ang_sin_rad < 0:
      ball_ang_rad *= -1.
    goal_ang_sin_rad = current_state[13]
    goal_ang_cos_rad = current_state[14]
    goal_ang_rad = math.acos(goal_ang_cos_rad)
    if goal_ang_sin_rad < 0:
      goal_ang_rad *= -1.
    alpha = abs(ball_ang_rad - goal_ang_rad)
    # By law of cosines. Alpha is angle between ball and goal
    ball_dist_goal = math.sqrt(ball_dist*ball_dist + goal_dist*goal_dist - \
                                2.*ball_dist*goal_dist*math.cos(alpha))
    # Passing update
    ball_vel_valid = current_state[54]
    ball_vel = current_state[55]
    if ball_vel_valid and ball_vel > kPassVelThreshold:
      self.pass_active = True
    if self.steps > 0:
      self.ball_prox_delta = self.ball_proximity - self.old_ball_prox
      self.kickable_delta = kickable - self.old_kickable
      self.ball_dist_goal_delta = ball_dist_goal - self.old_ball_dist_goal
    
    # replace old para
    self.old_ball_prox = self.ball_proximity
    self.old_kickable = kickable
    self.old_ball_dist_goal = ball_dist_goal

    if self.episode_over:
      # an episode over
      self.ball_prox_delta = 0
      module_logger.debug("episode_over:")
      self.kickable_delta = 0
      self.ball_dist_goal_delta = 0

    self.old_player_on_ball = self.player_on_ball
    self.player_on_ball = hfo_env.playerOnBall()
    #  VLOG(1) << "Player On Ball: " << player_on_ball.unum;
    self.steps+=1
  def reward(self):
    moveToBallReward = 5 * self.move_to_ball_reward()
    kickToGoalReward = 3. * self.kick_to_goal_reward()
    # passReward = 3. * self.pass_reward()
    eotReward = self.EOT_reward()
    reward = moveToBallReward + kickToGoalReward + eotReward
    self.extrinsic_reward += eotReward
    return reward


  # Reward for moving to ball and getting kickable. Ends episode once
  ## kickable is attained.
  def move_to_ball_reward(self):
    reward = 0
    # if self.player_on_ball.unum < 0 or self.player_on_ball.unum == self.our_unum:
    reward += 5*self.ball_prox_delta
    # module_logger.debug("reword: %f"%(self.ball_prox_delta))
    if self.kickable_delta >= 1 and not self.got_kickable_reward:
      reward += 1.0
      self.got_kickable_reward = True
    return reward

  # Reward for kicking ball towards the goal
  def kick_to_goal_reward(self):
    if self.player_on_ball == self.our_unum:
      return -self.ball_dist_goal_delta
    elif self.got_kickable_reward: # We have passed to teammate
      return 0.2 * -self.ball_dist_goal_delta
    return 0

  def EOT_reward(self):
    if self.status == hfo.GOAL:
    #  CHECK(old_player_on_ball.side == LEFT) << "Unexpected side: "
                                           #  << old_player_on_ball.side;
      if self.player_on_ball == self.our_unum:
      #  VLOG(1) << "We Scored!";
        return 5
      else: 
      #  VLOG(1) << "Teammate Scored!";
        return 1
    elif self.status == hfo.CAPTURED_BY_DEFENSE:
      return 0
    return 0

  def pass_reward(self):
    if self.pass_active and self.player_on_ball > 0 and not self.player_on_ball.unum == self.old_player_on_ball.unum:
      self.pass_active = False
    #  LOG(INFO) << "Unum " << our_unum << " self.steps " << self.steps << " got pass reward!";
      return 1
    return 0

